/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPose3DQuat>;

class Pose3DQuatTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	void test_compose(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x2, double y2, double z2, double yaw2,
		double pitch2, double roll2)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);

		const CPose3D p1_c_p2 = p1 + p2;
		const CPose3D p1_i_p2 = p1 - p2;

		CPose3DQuat q1 = CPose3DQuat(p1);
		CPose3DQuat q2 = CPose3DQuat(p2);

		CPose3DQuat q1_c_q2 = q1 + q2;
		CPose3DQuat q1_i_q2 = q1 - q2;

		CPose3D p_q1_c_q2 = CPose3D(q1_c_q2);
		CPose3D p_q1_i_q2 = CPose3D(q1_i_q2);

		EXPECT_NEAR(
			0,
			(p1_c_p2.asVectorVal() - p_q1_c_q2.asVectorVal())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1_c_p2: " << p1_c_p2 << endl
			<< "q1_c_p2: " << p_q1_c_q2 << endl;

		EXPECT_NEAR(
			0,
			(p1_i_p2.asVectorVal() - p_q1_i_q2.asVectorVal())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1_i_p2: " << p1_i_p2 << endl
			<< "q1_i_p2: " << p_q1_i_q2 << endl;

		// Test + operator: trg new var
		{
			CPose3DQuat C = q1;
			CPose3DQuat A = C + q2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
				1e-6);
		}
		// Test + operator: trg same var
		{
			CPose3DQuat A = q1;
			A = A + q2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
				1e-6);
		}
		// Test =+ operator
		{
			CPose3DQuat A = q1;
			A += q2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
				1e-6);
		}
	}

	void test_composePoint(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3DQuat q1(p1);
		const CPoint3D p(x, y, z);

		CPoint3D p1_plus_p = p1 + p;
		CPoint3D q1_plus_p = q1 + p;

		EXPECT_NEAR(
			0,
			(p1_plus_p.asVectorVal() - q1_plus_p.asVectorVal())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1: " << p1 << endl
			<< "q1: " << q1 << endl
			<< "p: " << p << endl
			<< "p1_plus_p: " << p1_plus_p << endl
			<< "q1_plus_p: " << q1_plus_p << endl;
	}

	static void func_compose_point(
		const CVectorFixedDouble<7 + 3>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
	{
		CPose3DQuat q(
			x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
		q.quat().normalize();
		const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
		const CPoint3D pp = q + p;
		for (int i = 0; i < 3; i++)
			Y[i] = pp[i];
	}

	void test_composePointJacob(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);

		TPoint3D l;
		q1.composePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);

		// Numerical approximation:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<7 + 3> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = q1[i];
			x_mean[7 + 0] = x;
			x_mean[7 + 1] = y;
			x_mean[7 + 2] = z;

			double DUMMY = 0;
			CVectorFixedDouble<7 + 3> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<7 + 3>& x, const double& dummy,
					CVectorFixedDouble<3>& Y)>(&func_compose_point),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.asEigen().block<3, 7>(0, 0);
			num_df_dpoint = numJacobs.asEigen().block<3, 3>(0, 7);
		}

		// Compare:
		EXPECT_NEAR(
			0,
			(df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpoint << endl
			<< "Error: " << endl
			<< df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_invComposePoint(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3DQuat q1(p1);
		const CPoint3D p(x, y, z);

		CPoint3D p_minus_p1 = p - p1;
		CPoint3D p_minus_q1 = p - q1;

		CPoint3D p_rec = q1 + p_minus_q1;

		EXPECT_NEAR(
			0,
			(p_minus_p1.asVectorVal() - p_minus_q1.asVectorVal())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p_minus_p1: " << p_minus_p1 << endl
			<< "p_minus_q1: " << p_minus_q1 << endl;

		EXPECT_NEAR(
			0, (p_rec.asVectorVal() - p.asVectorVal()).array().abs().sum(),
			1e-5)
			<< "p_rec: " << p_rec << endl
			<< "p: " << p << endl;
	}

	static void func_inv_compose_point(
		const CVectorFixedDouble<7 + 3>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
	{
		CPose3DQuat q(
			x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
		q.quat().normalize();
		const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
		const CPoint3D pp = p - q;
		Y[0] = pp.x();
		Y[1] = pp.y();
		Y[2] = pp.z();
	}

	void test_invComposePointJacob(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);

		TPoint3D l;
		q1.inverseComposePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);

		// Also check the returned point, not just the jacobian:
		TPoint3D theorical;
		{
			const double qr = q1.quat().r();
			const double qx = q1.quat().x();
			const double qy = q1.quat().y();
			const double qz = q1.quat().z();
			const double Ax = x - x1;  // ax  -  x;
			const double Ay = y - y1;  // ay  -  y;
			const double Az = z - z1;  // az  -  z;
			theorical.x = Ax + 2 * (Ay) * (qr * qz + qx * qy) -
				2 * (Az) * (qr * qy - qx * qz) -
				2 * (square(qy) + square(qz)) * (Ax);
			theorical.y = Ay - 2 * (Ax) * (qr * qz - qx * qy) +
				2 * (Az) * (qr * qx + qy * qz) -
				2 * (square(qx) + square(qz)) * (Ay);
			theorical.z = Az + 2 * (Ax) * (qr * qy + qx * qz) -
				2 * (Ay) * (qr * qx - qy * qz) -
				2 * (square(qx) + square(qy)) * (Az);
		}
		EXPECT_NEAR(theorical.x, l.x, 1e-5);
		EXPECT_NEAR(theorical.y, l.y, 1e-5);
		EXPECT_NEAR(theorical.z, l.z, 1e-5);

		// Numerical approximation:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<7 + 3> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = q1[i];
			x_mean[7 + 0] = x;
			x_mean[7 + 1] = y;
			x_mean[7 + 2] = z;

			double DUMMY = 0;
			CVectorFixedDouble<7 + 3> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<7 + 3>& x, const double& dummy,
					CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.block<3, 7>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 7);
		}

		// Compare:
		EXPECT_NEAR(
			0,
			(df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
			<< "p:  " << p << endl
			<< "local:  " << l << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpoint.asEigen() << endl
			<< "Error: " << endl
			<< df_dpoint - num_df_dpoint << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
			<< "p:  " << p << endl
			<< "local:  " << l << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_fromYPRAndBack(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3DQuat q1(p1);
		const CPose3D p1r = CPose3D(q1);

		EXPECT_NEAR(
			0,
			(p1.getHomogeneousMatrixVal<CMatrixDouble44>() -
			 q1.getHomogeneousMatrixVal<CMatrixDouble44>())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< p1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;

		EXPECT_NEAR(
			0, (p1.asVectorVal() - p1r.asVectorVal()).array().abs().sum(), 1e-5)
			<< "p1: " << p1 << endl
			<< "q1: " << q1 << endl
			<< "p1r: " << p1r << endl;
	}

	void test_unaryInverse(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3D p1_inv = -p1;

		const CPose3DQuat q1(p1);
		const CPose3DQuat q1_inv = -q1;

		EXPECT_NEAR(
			0,
			(p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() -
			 q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
	}

	void test_copy(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);

		const CPose3DQuat q1(p1);
		const CPose3DQuat q2 = q1;

		EXPECT_NEAR(
			0,
			(q1.getHomogeneousMatrixVal<CMatrixDouble44>() -
			 q2.getHomogeneousMatrixVal<CMatrixDouble44>())
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
			<< q2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
	}

	void test_composeAndInvComposePoint(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
		TPoint3D pp(x, y, z), aux;
		q1.composePoint(x, y, z, pp.x, pp.y, pp.z);
		q1.inverseComposePoint(pp.x, pp.y, pp.z, aux.x, aux.y, aux.z);

		EXPECT_NEAR(x, aux.x, 1e-7);
		EXPECT_NEAR(y, aux.y, 1e-7);
		EXPECT_NEAR(z, aux.z, 1e-7);
	}

	void test_composePoint_vs_CPose3D(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3DQuat q1(p1);
		TPoint3D pt1, pt2;
		p1.composePoint(x, y, z, pt1.x, pt1.y, pt1.z);
		q1.composePoint(x, y, z, pt2.x, pt2.y, pt2.z);

		EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
		EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
		EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
	}

	void test_invComposePoint_vs_CPose3D(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
		const CPose3DQuat q1(p1);
		TPoint3D pt1, pt2;
		p1.inverseComposePoint(x, y, z, pt1.x, pt1.y, pt1.z);
		q1.inverseComposePoint(x, y, z, pt2.x, pt2.y, pt2.z);

		EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
		EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
		EXPECT_NEAR(pt1.z, pt2.z, 1e-7);

		{
			CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);

			double gx = x, gy = y, gz = z;

			double dist, yaw, pitch;
			p1.sphericalCoordinates(TPoint3D(gx, gy, gz), dist, yaw, pitch);

			double lx, ly, lz;
			p1.inverseComposePoint(gx, gy, gz, lx, ly, lz);

			double lx2, ly2, lz2;
			q.inverseComposePoint(gx, gy, gz, lx2, ly2, lz2);

			EXPECT_NEAR(lx, lx2, 1e-7);
			EXPECT_NEAR(ly, ly2, 1e-7);
			EXPECT_NEAR(lz, lz2, 1e-7);
		}
	}

	static void func_spherical_coords(
		const CVectorFixedDouble<7 + 3>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
	{
		CPose3DQuat q(
			x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
		q.quat().normalize();
		const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
		q.sphericalCoordinates(p, Y[0], Y[1], Y[2]);
	}

	void test_sphericalCoords(
		double x1, double y1, double z1, double yaw1, double pitch1,
		double roll1, double x, double y, double z)
	{
		const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
		const TPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);

		double hr, hy, hp;
		q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);

		// Numerical approximation:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<7 + 3> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = q1[i];
			x_mean[7 + 0] = x;
			x_mean[7 + 1] = y;
			x_mean[7 + 2] = z;

			double DUMMY = 0;
			CVectorFixedDouble<7 + 3> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<7 + 3>& x, const double& dummy,
					CVectorFixedDouble<3>& Y)>(&func_spherical_coords),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.block<3, 7>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 7);
		}

		// Compare:
		EXPECT_NEAR(
			0,
			(df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpoint.asEigen() << endl
			<< "Error: " << endl
			<< df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	static void func_normalizeJacob(
		const CVectorFixedDouble<4>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<4>& Y)
	{
		CQuaternionDouble q;
		for (int i = 0; i < 4; i++)
			q[i] = x[i];
		q.normalize();
		for (int i = 0; i < 4; i++)
			Y[i] = q[i];
	}

	void test_normalizeJacob(double yaw1, double pitch1, double roll1)
	{
		const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
		CQuaternionDouble q1;
		pp.getAsQuaternion(q1);

		CMatrixFixed<double, 4, 4> df_dpose(UNINITIALIZED_MATRIX);
		q1.normalizationJacobian(df_dpose);

		// Numerical approximation:
		CMatrixFixed<double, 4, 4> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<4> x_mean;
			for (int i = 0; i < 4; i++)
				x_mean[i] = q1[i];

			double DUMMY = 0;
			CVectorFixedDouble<4> x_incrs;
			x_incrs.fill(1e-5);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<4>& x, const double& dummy,
					CVectorFixedDouble<4>& Y)>(&func_normalizeJacob),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.block<4, 4>(0, 0);
		}

		// Compare:
		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "q1: " << q1 << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}
};

// Check yaw-pitch-roll [rad] vs its [qx,qy,qz,qw] representation:
static void quat_vs_YPR(
	double yaw, double pitch, double roll, double qx, double qy, double qz,
	double qw, const std::string& sRotMat)
{
	const double eps = 1e-4;

	// First, test Yaw-pitch-roll to Rot matrix:
	const mrpt::poses::CPose3D p(0, 0, 0, yaw, pitch, roll);
	mrpt::math::CMatrixDouble33 R_gt;
	std::stringstream sErr;
	if (!R_gt.fromMatlabStringFormat(sRotMat, sErr))
		GTEST_FAIL() << "Incorrect R_gt matrix: '" << sRotMat
					 << "'. Error: " << sErr.str() << "\n";
	EXPECT_EQ(R_gt.cols(), 3) << " for: sRotMat='" << sRotMat << "'\n";
	EXPECT_EQ(R_gt.rows(), 3) << " for: sRotMat='" << sRotMat << "'\n";

	EXPECT_NEAR(
		0,
		(R_gt.asEigen() - p.getRotationMatrix().asEigen())
			.array()
			.abs()
			.maxCoeff(),
		eps)
		<< "R_gt=\n"
		<< R_gt << "\np.R=\n"
		<< p.getRotationMatrix() << std::endl;

	// Convert to quat:
	const auto q_gt = mrpt::math::CQuaternionDouble(qw, qx, qy, qz);
	mrpt::math::CQuaternionDouble q;
	p.getAsQuaternion(q);

	if (std::abs(q.w() - q_gt.w()) > eps || std::abs(q.x() - q_gt.x()) > eps ||
		std::abs(q.y() - q_gt.y()) > eps || std::abs(q.z() - q_gt.z()) > eps)
	{
		GTEST_FAIL() << "q = " << q.asString()
					 << "\nExpected = " << q_gt.asString() << "\n";
	}

	auto R = q.rotationMatrix<mrpt::math::CMatrixDouble33>();
	EXPECT_NEAR(
		0,
		(R.asEigen() - p.getRotationMatrix().asEigen())
			.array()
			.abs()
			.maxCoeff(),
		eps)
		<< "q.R=\n"
		<< R << "\np.R=" << p.getRotationMatrix() << std::endl;
}

// Check yaw-pitch-roll vs its [qx,qy,qz,qw] representation:
TEST(QuatTests, Quat_vs_YPR)
{
	// Ground truth values obtained from:
	// https://www.andre-gaschler.com/rotationconverter/
	// Using: ZYX Euler angles convention

	quat_vs_YPR(
		0.0_deg, 0.0_deg, 0.0_deg,	// Input Yaw-pitch-roll
		0, 0, 0, 1,	 // Expected quaternion
		"[  1.0000000  0.0000000  0.0000000; "
		"   0.0000000  1.0000000  0.0000000; "
		"   0.0000000  0.0000000  1.0000000 ]");
	quat_vs_YPR(
		90.0_deg, 0.0_deg, 0.0_deg,	 // Input Yaw-pitch-roll
		0, 0, 0.7071068, 0.7071068,	 // Expected quaternion
		"[  0.0000000 -1.0000000  0.0000000;"
		"   1.0000000  0.0000000  0.0000000;"
		"   0.0000000  0.0000000  1.0000000 ]");
	quat_vs_YPR(
		30.0_deg, 10.0_deg, 60.0_deg,  // Input Yaw-pitch-roll
		0.4615897, 0.2018243, 0.1811979, 0.8446119,	 // Expected quaternion
		"[  0.8528686 -0.1197639  0.5082046;"
		"   0.4924039  0.5082046 -0.7065880;"
		"  -0.1736482  0.8528686  0.4924039 ]");
	quat_vs_YPR(
		-10.0_deg, -20.0_deg, -30.0_deg,  // Input Yaw-pitch-roll
		-0.2685358, -0.1448781, -0.1276794, 0.9437144,	// Expected quaternion
		"[  0.9254166  0.3187958 -0.2048741;"
		"  -0.1631759  0.8231729  0.5438381;"
		"   0.3420202 -0.4698463  0.8137977 ]");
	quat_vs_YPR(
		-179.9995949_deg, -90.0_deg, 0.0_deg,  // Input Yaw-pitch-roll
		0.7071068, 0, 0.7071068, -0.000005,	 // Expected quaternion
		"[  0.0000000  0.0000071  1.0000000;"
		"  -0.0000071 -1.0000000  0.0000071;"
		"   1.0000000 -0.0000071  0.0000000 ]");
}

TEST_F(Pose3DQuatTests, FromYPRAndBack)
{
	test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
	test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
}

TEST_F(Pose3DQuatTests, UnaryInverse)
{
	test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
	test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
}

TEST_F(Pose3DQuatTests, CopyOperator)
{
	test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
	test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
	test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
	test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
}

TEST_F(Pose3DQuatTests, Compose)
{
	test_compose(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
		-5.0_deg, 25.0_deg);

	test_compose(
		25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
		20.0_deg, 9.0_deg, 0.0_deg);
}

TEST_F(Pose3DQuatTests, ComposeWithPoint)
{
	test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_composePoint(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_composePoint(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
{
	test_composePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePointJacob(
		1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_composePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_composePointJacob(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_composePointJacob(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, InvComposeWithPoint)
{
	test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_invComposePoint(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_invComposePoint(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
{
	test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
	test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_invComposePointJacob(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
{
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_composeAndInvComposePoint(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
{
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_composePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
{
	test_invComposePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_invComposePoint_vs_CPose3D(
		1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);

	for (size_t i = 0; i < 10; i++)
	{
		std::vector<double> v(9);
		mrpt::random::getRandomGenerator().drawGaussian1DVector(v, 0, 1);
		test_invComposePoint_vs_CPose3D(
			v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
	}
}

TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
{
	test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
	test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
	test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
	test_sphericalCoords(
		1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
	test_sphericalCoords(
		1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
}

TEST_F(Pose3DQuatTests, NormalizationJacobian)
{
	test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
	test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
	test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
	test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
	test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
	test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);
}
